﻿using System;

namespace MathService.Values
{
    [Serializable]
    public class DifferentialWithAveraging : IResetable, IValue<double>
    {
        protected double f_LastValue;
        protected TimeSpan f_LastTime;

        public double Tau { get; set; }
        public double Value { get; set; }
        public bool Initialized { get; protected set; }

        public DifferentialWithAveraging() { }
        public DifferentialWithAveraging(double Tau) { this.Tau = Tau; }

        //public DifferentialWithAveraging(double Tau, double Value) : this(Tau) { Add(Value); }

        public void Reset() { Initialized = false; }

        public virtual double Add(double value)
        {
            var t = DateTime.Now.TimeOfDay;

            if(!Initialized)
            {
                f_LastValue = value;
                f_LastTime = t;
                Initialized = true;
                return value;
            }

            var dv = value - f_LastValue;
            var lv_DeltaTime = t - f_LastTime;
            var dt = lv_DeltaTime.TotalSeconds;

            if(dt == 0) return Value;
            f_LastValue = Tau == 0 ? value : f_LastValue + (dv *= dt / Tau);

            f_LastTime = t;
            return Value = dv / dt;
        }

        public static implicit operator double(DifferentialWithAveraging D)
        {
            return D.Value;
        }
    }

    [Serializable]
    public class DifferentialWithAveraging2 : DifferentialWithAveraging
    {
        protected double f_LastV1;
        //protected double f_LastVf;

        public override double Add(double value)
        {
            var t = DateTime.Now.TimeOfDay;
            if(!Initialized)
            {
                f_LastValue = value;
                f_LastV1 = 0;
                //f_LastVf = 0.0;
                Value = 0;
                f_LastTime = t;
                Initialized = true;
                return value;
            }

            var dt = (t - f_LastTime).TotalSeconds;
            if(dt == 0) return Value;

            var dv = (value - f_LastV1) / Tau;
            var dvdt = f_LastV1 + (dv * dt);
            var lv_IntV = Value + (((dv - Value) / Tau) * dt);
            //var result = dvdt + (Tau * lv_IntV);

            f_LastTime = t;
            f_LastV1 = dvdt;
            f_LastValue = value;
            //f_LastVf = result;
            Value = lv_IntV;

            return lv_IntV;
        }
    }


}
